#include "stdafx.h"
#include "ParticleSwarm.h"
#include <limits>
#include <algorithm>
using namespace std;


static inline float randFloat( float minV, float mavX )
{
	float n = (float)rand () / RAND_MAX;
	return minV + (mavX - minV) * n;
}

ParticleSwarm::ParticleSwarm(FitnessFunc fitnessFunc, size_t dim, size_t nParticles, float minPos, float maxPos) : 
	fitnessFunc(fitnessFunc),
	dim(dim), 
	minPos(minPos), 
	maxPos(maxPos),
	maxV(maxPos - minPos), 
	bestParticle(0), 
	iterationCount(0),
	c1(2.05f),
	c2(2.05f),
	ksi(0.729f)
{
	particles.resize(nParticles);
	for(size_t i = 0; i < particles.size(); i++)
	{
		Particle& p = particles[i];
		p.pos.resize(dim);
		p.v.resize(dim);
		p.bestPos.resize(dim);
		p.bestF = -numeric_limits<float>::max();
		for(size_t j = 0; j < dim; j++)
		{
			p.pos[j] = randFloat(minPos, maxPos);
			p.v[j] = randFloat(-maxV, maxV);
		}
	}
	calcFitness();
}

void ParticleSwarm::calcFitness()
{
	for(size_t i = 0; i < particles.size(); i++)
	{
		Particle& p = particles[i];
		float f = fitnessFunc(&p.pos[0]);
		if(f > p.bestF)
		{
			p.bestF = f;
			copy(p.pos.begin(), p.pos.end(), p.bestPos.begin());
		}
	}

	bestParticle = 0;
	for(size_t i = 1; i < particles.size(); i++)
	{
		if(particles[i].bestF > particles[bestParticle].bestF) bestParticle = i;
	}
}

void ParticleSwarm::move()
{
	Particle& pBest = particles[bestParticle];
	for(size_t i = 0; i < particles.size(); i++)
	{
		Particle& p = particles[i];
		for(size_t j = 0; j < dim; j++)
		{
			float r1 = randFloat(0.f, c1);
			float r2 = randFloat(0.f, c2);
			p.v[j] = ksi*(p.v[j] + r1*(p.bestPos[j] - p.pos[j]) + r2*(pBest.bestPos[j] - p.pos[j]));
			p.pos[j] += p.v[j];
		}
	}
}

void ParticleSwarm::step()
{
	move();
	calcFitness();
	iterationCount++;
}

const float* ParticleSwarm::getBest() const
{
	return &particles[bestParticle].bestPos[0];
}

float ParticleSwarm::getBestFitness() const
{
	return particles[bestParticle].bestF;
}

size_t ParticleSwarm::getIterationCount() const
{
	return iterationCount;
}

template<class T>
static void binaryWrite(std::ostream& f, const T& a)
{
	f.write((const char*)&a, sizeof(a));
}

template<class T>
static void binaryWrite(std::ostream& f, const std::vector<T>& a)
{
	f.write((const char*)&a[0], sizeof(T)*a.size());
}

template<class T>
static void binaryRead(std::istream& f, T& a)
{
	f.read((char*)&a, sizeof(a));
}

template<class T>
static void binaryRead(std::istream& f, std::vector<T>& a)
{
	f.read((char*)&a[0], sizeof(T)*a.size());
}

static const int header = 414424580;

void ParticleSwarm::save(std::ostream& f)
{
	binaryWrite(f, header);
	binaryWrite(f, iterationCount);
	binaryWrite(f, bestParticle);
	for(size_t i = 0; i < particles.size(); i++)
	{
		Particle& p = particles[i];
		binaryWrite(f, p.bestF);
		binaryWrite(f, p.bestPos);
		binaryWrite(f, p.pos);
		binaryWrite(f, p.v);

	}
}

void ParticleSwarm::load(std::istream& f)
{
	int h = 0;
	binaryRead(f, h);
	if(h != header) return;
	binaryRead(f, iterationCount);
	binaryRead(f, bestParticle);
	for(size_t i = 0; i < particles.size(); i++)
	{
		Particle& p = particles[i];
		binaryRead(f, p.bestF);
		binaryRead(f, p.bestPos);
		binaryRead(f, p.pos);
		binaryRead(f, p.v);
	}
}
